// Copyright (c) 2012, Hazen Eckert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the The University of Texas at Dallas nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT DALLAS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <ros/ros.h>
#include <auv_passive_sonar_driver/passive_sonar_driver.h>
#include <auv_passive_sonar_driver/passive_sonar_driver_node.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <dynamic_reconfigure/server.h>
#include <auv_passive_sonar_driver/auvConfig.h>
#include <auv_msgs/PassiveSonar.h>

passive_sonar_driver_node::passive_sonar_driver_node(ros::NodeHandle h):auv_driver_node(h), server()
{
    state = auv_driver_node::CLOSED;
    
    driver = new passive_sonar_driver;
    
  	dynamic_reconfigure::Server<auv_passive_sonar_driver::auvConfig>::CallbackType f;
  	f = boost::bind(&passive_sonar_driver_node::reconfigureCallback, this, _1, _2);
    server.setCallback(f);
    
    private_nh.param<std::string>("port", port, "/dev/passive_sonar");
    private_nh.param<int>("SampleFrequency", SAMPLE_FREQUENCY, 14745600);
    private_nh.param<int>("SpeedOfSound", SPEED_OF_SOUND, 1500);
    private_nh.param<double>("Hydrophone_0_x", hydrophone_0_x, 0.59000);
    private_nh.param<double>("Hydrophone_0_y", hydrophone_0_y, 0.0);
    private_nh.param<double>("Hydrophone_1_x", hydrophone_1_x, 0.0);
    private_nh.param<double>("Hydrophone_1_y", hydrophone_1_y, 0.133);
    private_nh.param<double>("Hydrophone_2_x", hydrophone_2_x, 0.0);
    private_nh.param<double>("Hydrophone_2_y", hydrophone_2_y, -0.133);
    
    
    try {
    	driver->open(port.c_str(), 115200);
    	state = auv_driver_node::OPENED;
    } catch (int e) {
    	if (e == 19){
    		ROS_ERROR("Open FAILED");
    	}
    	state = auv_driver_node::CLOSED;
    }
    
    private_nh.param<bool>("psonarMode", psonarMode, true);
    
    if (driver->isOpened() && psonarMode == true) {
        if (((passive_sonar_driver *)driver)->setCompetitionMode()!= 0) {
            ROS_ERROR("Set competition mode failed");
        }
    } else if (driver->isOpened() && psonarMode == false) {
        if (((passive_sonar_driver *)driver)->setPracticeMode() != 0) {
            ROS_ERROR("Set practice mode failed");
        }
    } else {
        ROS_ERROR("Set mode failed");
    }
    
    psonar_pub = nh.advertise<auv_msgs::PassiveSonar>("passiveSonar", 10);
    
}

passive_sonar_driver_node::~passive_sonar_driver_node()
{
    delete driver;
}

void passive_sonar_driver_node::reconfigureCallback(auv_passive_sonar_driver::auvConfig &config, uint32_t level)
{
    if (((std::string)config.port_name).compare(port) != 0) {
        if (state > auv_driver_node::CLOSED) {
       		driver->close();
       		state = auv_driver_node::CLOSED;
       	}
       	
        try {
            driver->open(((std::string)config.port_name).c_str(), 115200);
            state = auv_driver_node::OPENED;
        } catch (int e) {
            if (e == 19){
                ROS_ERROR("Open FAILED");
            }
            state = auv_driver_node::CLOSED;
        }
    }
    
    if (driver->isOpened() && config.competitionMode == true) {
        if (((passive_sonar_driver *)driver)->setCompetitionMode()!= 0) {
            ROS_ERROR("Set competition mode failed");
        } 
    } else if (driver->isOpened() && config.competitionMode == false) {
        if (((passive_sonar_driver *)driver)->setPracticeMode() != 0) {
            ROS_ERROR("Set practice mode failed");
        }
    } else {
        ROS_ERROR("Set mode failed");
    }
    
}

void passive_sonar_driver_node::spin()
{
    ros::Rate loop_rate(50);
    
    bool isFirstPing = true;
    
    uint32_t timeOfPing_old, timeOfMessageSent_old, ourTime;
    timeOfPing_old = timeOfMessageSent_old = ourTime = 0;
    int16_t timeDifferenceBetween1and0_old, timeDifferenceBetween2and0_old;
    timeDifferenceBetween1and0_old = timeDifferenceBetween2and0_old = 0x7FFF;
    
    double distanceFrom1to0 = sqrt(pow(hydrophone_0_x - hydrophone_1_x, 2) + pow(hydrophone_0_y - hydrophone_1_y, 2));
    double distanceFrom2to0 = sqrt(pow(hydrophone_0_x - hydrophone_2_x, 2) + pow(hydrophone_0_y - hydrophone_2_y, 2));
    double distanceFrom1to2 = sqrt(pow(hydrophone_2_x - hydrophone_1_x, 2) + pow(hydrophone_2_y - hydrophone_1_y, 2));
    
    double alpha = acos((pow(distanceFrom1to0, 2) + pow(distanceFrom2to0, 2) - pow(distanceFrom1to2, 2))/(2*distanceFrom1to0*distanceFrom2to0));
    double delta = atan2(hydrophone_0_y - hydrophone_1_y, hydrophone_0_x - hydrophone_2_x);
    delta = fmod(delta + M_PI*2, M_PI*2);
    
    double cosPhi = cos(alpha);
    double sinPhi = sin(alpha);
    
    while (!ros::isShuttingDown()) {
        loop_rate.sleep();
        if (!driver->isOpened()) {
            state = auv_driver_node::CLOSED;
        } else {
            int16_t timeDifferenceBetween1and0, timeDifferenceBetween2and0;
            uint32_t timeOfPing, timeOfMessageSent;
            if (((passive_sonar_driver *)driver)->getTimeDelay(&timeDifferenceBetween1and0, &timeDifferenceBetween2and0, &timeOfPing, &timeOfMessageSent) != 0) {
                ROS_ERROR("Get sonar data failed");
            } else {
                if (timeOfPing > timeOfMessageSent) {
                    ROS_ERROR("Get sonar data failed");
                }
                
                if (isFirstPing) {
                    if (timeDifferenceBetween1and0_old != 0x7FFF || timeDifferenceBetween2and0_old != 0x7FFF || timeOfMessageSent_old != 0) {
                        ROS_ERROR("It says it's the first ping but the variables aren't initialized");
                    }
                }
                
                bool boardHasRecievedPing = false;
                if (timeDifferenceBetween1and0 != 0x7FFF || timeDifferenceBetween2and0 != 0x7FFF) {
                    boardHasRecievedPing = true;
                }
                
                if (!boardHasRecievedPing) {
                    if (!isFirstPing) {
                        ROS_ERROR("The board never recieved a ping but we did");
                    } else {
                        timeOfMessageSent_old = timeOfMessageSent;
                        ourTime = (uint32_t)(ros::Time::now().toNSec()/100000); // units in 0.1 ms
                    }
                    continue;
                }
                
                if (isFirstPing) {
                    timeDifferenceBetween1and0_old = timeDifferenceBetween1and0;
                    timeDifferenceBetween2and0_old = timeDifferenceBetween2and0;
                    timeOfPing_old = timeOfPing;
                    timeOfMessageSent_old = timeOfMessageSent;
                    ourTime = (uint32_t)(ros::Time::now().toNSec()/100000);
                    isFirstPing = false;
                    continue;
                }
                
                if (timeOfPing < timeOfPing_old) {
                    ROS_ERROR("New ping time, but in the past...");
                    continue;
                }
                
                if (timeOfMessageSent < timeOfMessageSent_old) {
                    ROS_ERROR("New time of message, but in the past...");
                    continue;
                }
                
                if (timeOfPing == timeOfPing_old) {
                    if (timeDifferenceBetween1and0 != timeDifferenceBetween1and0_old || timeDifferenceBetween2and0 != timeDifferenceBetween2and0_old) {
                        ROS_ERROR("Same ping, different time diffs");
                    }
                    timeOfMessageSent_old = timeOfMessageSent;
                    ourTime = (uint32_t)(ros::Time::now().toNSec()/100000);
                    continue;
                }
                
                timeDifferenceBetween1and0_old = timeDifferenceBetween1and0;
                timeDifferenceBetween2and0_old = timeDifferenceBetween2and0;
                timeOfPing_old = timeOfPing;
                timeOfMessageSent_old = timeOfMessageSent;
                ourTime = (uint32_t)(ros::Time::now().toNSec()/100000);
                
                // Now we know we have a valid new ping 
                
                double timeDelayOfArrivalBetween1and0 = timeDifferenceBetween1and0/SAMPLE_FREQUENCY;
                double timeDelayOfArrivalBetween2and0 = timeDifferenceBetween2and0/SAMPLE_FREQUENCY;
                
                double differenceInDistanceFromPingerFor1and0 = timeDelayOfArrivalBetween1and0*SPEED_OF_SOUND;
                double differenceInDistanceFromPingerFor2and0 = timeDelayOfArrivalBetween2and0*SPEED_OF_SOUND;
                
                double z = differenceInDistanceFromPingerFor1and0/distanceFrom1to0;
                double y = (differenceInDistanceFromPingerFor2and0/distanceFrom2to0 - z*cosPhi)/sinPhi;
                double x = sqrt(fmax(0, 1-y*y-z*z));
                
                double bearing = fmod(atan2(y,z) + delta + M_PI, 2*M_PI) - M_PI;
                double elevation = fmod(atan2(x, sqrt(y*y + z*z)) + M_PI, 2*M_PI) - M_PI;
                
                auv_msgs::PassiveSonar msg;
                msg.bearing = bearing;
                msg.elevation = elevation;
                psonar_pub.publish(msg);
            }
        }
        
    }
}



int main(int argc, char **argv)
{
    ros::init(argc, argv, "auv_passive_sonar_driver");
    
    ros::NodeHandle n;

    passive_sonar_driver_node node(n);
    
    node.spin();

    return 0;
}
